%author: wxj233
%time: 2023.11.10 10:00
%function:  轨迹


classdef Track < handle
    %TRACK 此处显示有关此类的摘要
    %   此处显示详细说明
    
    properties
        id  % 轨迹id
        fps  % feature points特征点[fp1, fp2, ...]单个fp包含{points, Xk_e, Pk_ee, fplife}
        lifeMax  % 轨迹最大生命值

        validFpNum  % 本次跟新有效量测数量
        tlife  % track life,轨迹当前生命值，每记忆一次减一
        mNum  % 量测数量(measure Number),有效量测次数,每有效关联一次加一
        len  % 轨迹总长度,每迭代一次加一
        gV  % group V群速度,用于记录群状态[vx, vy]，速度为有效关联的特征点的速度均值
        vargV  % 群速度方差[varVx, varVy]

        points  % 群状态记录[t, x, vx, y, vy; ...]，位置取均值
        mfpTracer  % 该轨迹所属追踪器
    end
    
    methods
        function obj = Track(tlifeMax, mfpTracer)
            %TRACK 构造此类的实例
            % tlifeMax: 轨迹最大生命值
            obj.id = obj.geId();  % 自动递增id
            obj.fps = struct([]);
            obj.lifeMax = tlifeMax;
            obj.mNum = 1;
            obj.len = 1;
            obj.validFpNum = 0;
            obj.points = [];
            obj.mfpTracer = mfpTracer;
        end
        

        function predict(obj, vp, dt, q)
            % 轨迹预测完成每个特征点的预测
            % vp: 自身速度在新的融合速度中所占的比例V_new = Ve + p(V' - Ve),V'代表自身速度，Ve代表平均速度
            % dt: 预测时间间隔
            % q: 过程噪声量级
            for i = 1:1:length(obj.fps)
                fp = obj.fps(i);
                fp.Xk_e([2,4],1) = obj.gV' + vp*(obj.gV' - fp.Xk_e([2,4],1));  % 融合速度

                varVx = fp.Pk_ee(2, 2);  % 融合前的vx的方差
                if fp.isValidAss
                    fp.Pk_ee(2, 2) = vp*vp*fp.Pk_ee(2, 2)+(1-vp)*(1-vp)*obj.vargV(1) + 2*vp*(1-vp)/obj.validFpNum*fp.Pk_ee(2, 2);  % 融合后的vx的方差
                else
                    fp.Pk_ee(2, 2) = vp*vp*fp.Pk_ee(2, 2)+(1-vp)*(1-vp)*obj.vargV(1);  % 融合后的vx的方差
                end
                fp.Pk_ee(1, 2) = fp.Pk_ee(1, 2)*sqrt(fp.Pk_ee(2, 2)/varVx);  % 更新协方差
                fp.Pk_ee(2, 1) = fp.Pk_ee(1, 2);

                varVy = fp.Pk_ee(4, 4);  % 融合前的vx的方差
                if fp.isValidAss
                    fp.Pk_ee(4, 4) = vp*vp*fp.Pk_ee(4, 4)+(1-vp)*(1-vp)*obj.vargV(2) + 2*vp*(1-vp)/obj.validFpNum*fp.Pk_ee(4, 4);  % 融合后的vy的方差
                else
                    fp.Pk_ee(4, 4) = vp*vp*fp.Pk_ee(4, 4)+(1-vp)*(1-vp)*obj.vargV(2);  % 融合后的vy的方差
                end
                fp.Pk_ee(3, 4) = fp.Pk_ee(3, 4)*sqrt(fp.Pk_ee(4, 4)/varVy);
                fp.Pk_ee(4, 3) = fp.Pk_ee(3, 4);

                fp.predict(dt, q);  % 同时更新了特征点内部状态
            end
        end


        function update(obj, r, nowTime)
            % 轨迹更新完成每个特征点的更新
            % r: 量测误差数量级
            % nowTime: 要更新的时刻
            for i = 1:1:length(obj.fps)
                fp = obj.fps(i);
                fp.update(fp.assP, r, nowTime);
            end
            
        end


        function fp = createFp(obj, p1, p2, r, fplifeMax)
            % 创建特征点
            % p1: t1时刻特征点[t1, x, y, id, cluster]
            % p2: t2时刻特征点[t2, x, y, id, cluster]
            % r: 测量误差方差
            % fplifeMax: 特征点最大生命值
            dt = p2(1) - p1(1);
            vx = (p2(2) - p1(2))/dt;
            vy = (p2(3) - p1(3))/dt;

            ps = [p1(1), p1(2), vx, p1(3), vy, p1(4), p1(5);
                  p2(1), p2(2), vx, p2(3), vy, p2(4), p2(5)];
            Xk_e = [p2(2), vx, p2(3), vy]';
            Pk_ee = [r, r/dt, 0, 0;
                     r/dt, 2*r/(dt*dt), 0, 0;
                     0, 0, r, r/dt;
                     0, 0, r/dt, 2*r/(dt*dt)];
            fp = FeaturePoint(fplifeMax, ps, Xk_e, Pk_ee, obj);
            obj.fps = [obj.fps, fp];
        end


        function calGroupState(obj, q)
            % 轨迹初始化时，在创建完成所有特征点后需要进行一次轨迹状态初始化
            % q: 过程噪声数量级
            if obj.len == 1  % 等于1就是第一次初始化
                state = zeros([2, 5]);
            else
                state = zeros([1, 5]);
            end
            obj.vargV = zeros([1, 2]);
            obj.validFpNum = 0;
            isvalid = false;
            for i = 1:1:length(obj.fps)
                if obj.fps(i).isValidAss
                    if obj.len == 1  % 等于1就是第一次初始化
                        state = state + obj.fps(i).points;
                    else
                        state = state + obj.fps(i).points(end, :);
                    end
                    obj.vargV = obj.vargV + [obj.fps(i).Pk_ee(2,2), obj.fps(i).Pk_ee(4,4)];
                    obj.validFpNum = obj.validFpNum + 1;
                    isvalid = true;
                end
            end
            
            if isvalid
                state = state/obj.validFpNum;
                obj.points = [obj.points; state(:, 1:5)];
    
                obj.gV = state(end, [3,5]);
                obj.vargV = obj.vargV/(obj.validFpNum*obj.validFpNum);
    
                obj.mNum = obj.mNum + 1;
                obj.len = obj.len + 1;
                obj.tlife = tlifeMax;
            else
                obj.len = obj.len + 1;
                obj.tlife = obj.tlife - 1;
                obj.vargV = obj.vargV + q*[1, 1];
                obj.gv = obj.gv;  % 群速度不变
            end
        end

        function deleteFp(obj, id)
            % 删除指定id的特征点
            for i = 1:1:length(obj.fps)
                if obj.fps(i).id == id
                    obj.fps(i) = [];
                    break;
                end
            end
        end

    end

    methods (Static)
        function id = geId()
          % 获取特征点唯一标识
          persistent ID;
          if isempty(ID)
             ID = 0;
          end
          ID = ID + 1;
          id = ID;
        end
    end
end

